/***************************************************************************
 *   Copyright (C) 2010 by Oleg Goncharov  *
 *   $EMAIL$                           *                          
 *                                                                         *
 *   This file is part of ChessVision.                                     *
 *                                                                         *
 *   ChessVision is free software; you can redistribute it and/or modify   *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 *   This program is distributed in the hope that it will be useful,       *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
 *   GNU General Public License for more details.                          *
 *                                                                         *
 *   You should have received a copy of the GNU General Public License     *
 *   along with this program; if not, write to the                         *
 *   Free Software Foundation, Inc.,                                       *
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
 ***************************************************************************/
#ifndef CCAMTRANSFORM_H
#define CCAMTRANSFORM_H

#include <list>
#include <opencv/cv.h>

#include "error/cerror.h"
#include "geomtypes.h"
#include "cimagesource.h"

/**
Преодразование координат 3D <-> изображение


	@author Goncharov Oleg <goncharovoi@yandex.ru>
*/
class CCamTransform {
	cv::Mat_<double> intrinsic;
	cv::Mat_<double> distortion; 
	cv::Mat_<double> rot_vect;
	cv::Mat_<double> trans_vect;	
	cv::Mat_<double> rot_mat;

	
	std::vector<cv::Point2f> i_points;
	std::vector<cv::Point3f> o_points;
	int history_length;
	
	void Reserve();
public:
	CCamTransform() { Reserve(); }
	CCamTransform(const char * file) { Reserve(); LoadIntrinsicParams(file); }
	~CCamTransform();
	
	bool CalibrateIntrisictParams(CImageSource& src, cv::Size pattern_size, double h, bool calc_distortion = true);
	void SaveIntrinsicParams(const char * file) const;
	void LoadIntrinsicParams(const char * file);
	void SaveTransform(const char * file) const;
	void LoadTransform(const char * file);
	
	inline void AddPair(const cv::Point2f& img, const cv::Point3f& obj);
	inline void DelPair();
	inline void NewPair(const cv::Point2f& img, const cv::Point3f& obj);
	inline void ClearPairs();
	
	inline void SetHistoryLength(int len);
	
	void CalcTransform();
	bool RecalcTransform();
	
	cv::Point2f ProjectPoint(const cv::Point3f& obj) const;
	cv::Point3f ReverseProjection(cv::Point2f img, HPlane3f plane) const;
	
	void RemoveDistortion(const cv::Mat& src, cv::Mat& dst) const;

	void PrintIParams() const;
	void PrintTParams() const;
};

void CCamTransform::SetHistoryLength(int len) { 
	if (len <= 3) throw Errors::CBadArgError(); 
	history_length = len; 
}

void CCamTransform::AddPair(const cv::Point2f& img, const cv::Point3f& obj) {
	i_points.push_back(img); 
	o_points.push_back(obj);
}

void CCamTransform::DelPair() {
	i_points.erase(i_points.begin());
	o_points.erase(o_points.begin());
}
	
void CCamTransform::NewPair(const cv::Point2f& img, const cv::Point3f& obj) {
	while (i_points.size() > history_length) {
		i_points.erase(i_points.begin());
		o_points.erase(o_points.begin());
	}
	AddPair(img, obj);
}

void CCamTransform::ClearPairs() {
	i_points.clear();
	o_points.clear();
}

#endif
